
#include"stdio.h"

// #include"imu_lidar_cali.h"
#include"undistort.h"

#include <ros/ros.h>
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

#include "pcl_conversions/pcl_conversions.h"
#include <pcl/io/ply_io.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>

#include"utils.h"
// #include"def.h"
#include"cali.h"

#include<string>
using namespace std;



/*
//caclulate the initial rotation matrix
int InitCali(string bagfile)
{
    pcl::PLYWriter ply_writer;

    string bag_filename = "/home/xdh/data/bag/lio-sam/identity/rotation.bag";
    std::cout<<bag_filename<<std::endl;

    pclomp::NormalDistributionsTransform<PT_TYPE, PT_TYPE> ompNdt;
    int num_threads = omp_get_max_threads();
    printf("max threads: %d \n", num_threads);
    ompNdt.setResolution(1.0);
    ompNdt.setNumThreads(num_threads-1);
    ompNdt.setNeighborhoodSearchMethod(pclomp::DIRECT7);

    rosbag::Bag bag;
    bag.open(bag_filename, rosbag::bagmode::Read);
    rosbag::View view(bag);
    const ::ros::Time begin_time = view.getBeginTime();
    const double duration_in_seconds =
        (view.getEndTime() - begin_time).toSec();
    cout << duration_in_seconds << endl;

    CLidarUnDistort undistort;
    // CImuLidarCali   cali;

    vector<LidarFrame> vecLidarFrame;
    vector<ImuData>    vecImuData;
    std::deque<sensor_msgs::PointCloud2> vecCloudMsg;
    for (const rosbag::MessageInstance &message : view)
    {
        std::string stopic = message.getTopic();

        if (message.isType<sensor_msgs::PointCloud2>())
        {
            const sensor_msgs::PointCloud2::ConstPtr &msg =
                message.instantiate<sensor_msgs::PointCloud2>();

            vecCloudMsg.push_back(*msg);

            if (vecCloudMsg.size() > 5)
            {
                sensor_msgs::PointCloud2 message = vecCloudMsg.front();
                vecCloudMsg.pop_front();

                double initStamp = message.header.stamp.toSec();

                pcl::PointCloud<PointXYZIRT> pcl_point_cloud;
                pcl::fromROSMsg(message, pcl_point_cloud);
                //ply_writer.write("pc-raw.ply", pcl_point_cloud);

                //printf("%d \n", pcl_point_cloud.size());
                //对当前点云进行运动畸变校正
                int res = 1; //undistort.Process(pcl_point_cloud, initStamp);
                //ply_writer.write("pc-deskew.ply", pcl_point_cloud);

                //将校正成功的点云进行slam处理
                if (res >= 0)
                {
                    pcl::PointCloud<PT_TYPE> deskewedPointCloud;
                    for (int i = 0; i < pcl_point_cloud.size(); i++)
                    {
                        PT_TYPE pt;
                        pt.x = pcl_point_cloud.points[i].x;
                        pt.y = pcl_point_cloud.points[i].y;
                        pt.z = pcl_point_cloud.points[i].z;
                        deskewedPointCloud.push_back(pt);
                    }

                    //voxel filter
                    VoxelFilter(deskewedPointCloud, 0.2);

                    LidarFrame lf;
                    lf.stamp = initStamp;
                    lf.cloud = deskewedPointCloud;
                    vecLidarFrame.push_back(lf);

                    if(vecLidarFrame.size()>100)
                        break;
                }
            }
        }

        if( message.isType<sensor_msgs::Imu>() )
        {
             const sensor_msgs::Imu::ConstPtr &msg =
                message.instantiate<sensor_msgs::Imu>();

            ImuData data;
            data.acc = Eigen::Vector3d(msg->linear_acceleration.x,
                                       msg->linear_acceleration.y,
                                       msg->linear_acceleration.z);
            data.gyr = Eigen::Vector3d(msg->angular_velocity.x,
                                       msg->angular_velocity.y,
                                       msg->angular_velocity.z);
            data.stamp = msg->header.stamp.toSec();
            vecImuData.push_back(data);
            undistort.AddImu(data);
        }
    }

    //对点云进行匹配, 生成旋转角度的约束
    vector<pair<Eigen::Quaterniond, Eigen::Quaterniond>> corres(0);
    for(int i=0; i<vecLidarFrame.size()-1; i++)
    {

        double l1_time = vecLidarFrame[i].stamp;
        double l2_time = vecLidarFrame[i + 1].stamp;

        if(l1_time<vecImuData[0].stamp)
            continue;    

        //根据imu的数据来预测相邻两帧点云之间的旋转矩阵
        float ax, ay, az;
        undistort.FindAngle(0, l1_time, ax, ay, az);
        Eigen::Affine3f ml1 = pcl::getTransformation(0, 0, 0, ax, ay, az);
        undistort.FindAngle(0, l2_time, ax, ay ,az);
        Eigen::Affine3f ml2 = pcl::getTransformation(0, 0, 0, ax, ay, az);
        Eigen::Matrix4f mr = (ml1.matrix()).inverse()*(ml2.matrix());

        //利用点云匹配来计算相对旋转
        ompNdt.setInputTarget(vecLidarFrame[i].cloud.makeShared());
        ompNdt.setInputSource(vecLidarFrame[i+1].cloud.makeShared());

        pcl::PointCloud<PT_TYPE>::Ptr aligned(new pcl::PointCloud<PT_TYPE>());
        ompNdt.align(*aligned, mr);
        float score = ompNdt.getFitnessScore();
        Eigen::Matrix4f mt = ompNdt.getFinalTransformation();
        Eigen::Matrix4d dmt = mt.cast<double>();

        // char srcFile[256];
        // char dstFile[256];
        // char guessFile[256];
        // char alignFile[256];
        // sprintf(srcFile, "%d-src.ply", i);
        // sprintf(dstFile, "%d-dst.ply", i);
        // sprintf(guessFile, "%d-guess.ply", i);
        // sprintf(alignFile, "%d-align.ply", i);
        // ply_writer.write(srcFile, vecLidarFrame[i+1].cloud);
        // ply_writer.write(dstFile, vecLidarFrame[i].cloud);
        // pcl::PointCloud<PT_TYPE> guessPC;
        // pcl::transformPointCloud(vecLidarFrame[i+1].cloud, guessPC, mr);
        // pcl::PointCloud<PT_TYPE> alignPC;
        // pcl::transformPointCloud(vecLidarFrame[i+1].cloud, alignPC, mt);
        // ply_writer.write(guessFile,guessPC);
        // ply_writer.write(alignFile,alignPC);
        
        if (ompNdt.hasConverged()) //&& score<1.0 )
        {
            Eigen::Quaterniond q_l2_l1 = Eigen::Quaterniond( dmt.block<3, 3>(0, 0)); //lidar

            //根据lio-sam的方法来计算imu相对旋转
            //float ax,ay,az;
            undistort.FindAngle(0, l1_time, ax, ay ,az);
            Eigen::Affine3f rotation = pcl::getTransformation(0, 0, 0, ax, ay, az);
            Eigen::Matrix3f m = (rotation.matrix()).block<3,3>(0,0);
            Eigen::Quaterniond q1 = Eigen::Quaterniond(m.cast<double>());

            undistort.FindAngle(0, l2_time, ax, ay ,az);
            rotation = pcl::getTransformation(0, 0, 0, ax, ay, az);
            m = (rotation.matrix()).block<3,3>(0,0);
            Eigen::Quaterniond q2 = Eigen::Quaterniond(m.cast<double>());
            Eigen::Quaterniond q_b2_b1 = q1.inverse()*q2; //imu

            double angle_distance = q2.angularDistance(q1) / 3.1415926 * 180;
            // if (angle_distance < MIN_ANGLE_DIS || angle_distance > MAX_ANGLE_DIS)
            //    continue;

            // double angle_distance = q_b2_b1.angularDistance(q_l2_l1) / 3.1415926 * 180;
            // if( angle_distance>0.5)
            //     continue;

            //根据开源标定代码的四元数插值来计算imu相对旋转
            Eigen::Quaterniond cq1,cq2;
            undistort.InterpolateQuaternion(0, l1_time, cq1);
            undistort.InterpolateQuaternion(0, l2_time, cq2);
            Eigen::Quaterniond q_b2_b1_c = cq1.inverse()*cq2; //imu

            std::cout<<i<<"...."<<"score: "<<score<<" angle dis: "<<angle_distance<<std::endl; 
            std::cout<<"lidar:"<<q_l2_l1.coeffs().transpose()<<std::endl;
            std::cout<<"imu:  "<<q_b2_b1.coeffs().transpose()<<std::endl;
            std::cout<<"imu:  "<<q_b2_b1_c.coeffs().transpose()<<std::endl;

            corres.push_back(move(pair<Eigen::Quaterniond, Eigen::Quaterniond>(q_l2_l1, q_b2_b1_c)));
        }
    }

    Eigen::Quaterniond q_l_b_ = solve(corres);
    tf::Matrix3x3 mat(tf::Quaternion(q_l_b_.x(), q_l_b_.y(), q_l_b_.z(), q_l_b_.w()));
    double roll,pitch,yaw;
    mat.getRPY(roll, pitch, yaw);
    double dpi = 1.0/3.1415926*180;
    Eigen::Matrix3d rot = Eigen::Matrix3d(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()));
    cout<<"rpy:"<<roll*dpi<<" "<<pitch*dpi<<" "<<yaw*dpi<<std::endl;
    cout << "rotation:" << rot.inverse() << endl;

    return 0;
}
*/

/*
examples:
    ./main_imu_lidar_cali  '/home/xdh/data/bag/lio-sam/identity/rotation.bag' /points_raw /imu_correct

*/
int main(int argc, char* argv[])
{
    printf("slam using ndt.... \n");

    string bag_filename = argv[1]; //"/home/xdh/data/bag/lio-sam/identity/rotation.bag";
    string lidar_topic = argv[2];  //激光的主题
    string imu_topic = argv[3];    //惯导的主题

    CImuLidarCali cali;
    cali.InitCali(bag_filename, lidar_topic, imu_topic);
    for(int k=0; k<1; k++)
    {
        cali.RefineCali(bag_filename, lidar_topic, imu_topic);
    }
    //cali.RefineCaliUsingScan2Map(bag_filename, lidar_topic, imu_topic);

    return 0;
}